/**
 * @FilePath: \ros2\src\agv_control\haredware\src\interface.cpp
 * @Date: 2025-03-14 08:09:34
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-20 10:53:59
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#include "hardware/interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp/time.hpp"
#include <std_msgs/msg/float64_multi_array.hpp>

namespace hardware
{
std_msgs::msg::Float64MultiArray msg_array;

hardware_interface::CallbackReturn SystemHardware::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) 
    return CallbackReturn::ERROR;
    
  hardware_node_ = std::make_shared<Hardware>("hardware_node");
  if (!hardware_node_->declare_parameters())
    return CallbackReturn::ERROR;

  int index = 0;
  // TODO(anyone): read parameters and initialize the hardware
  for (const hardware_interface::ComponentInfo & joint : info_.joints) {
    // chassis has exactly two states and one command interface on each joint
    if (joint.command_interfaces.size() != 2) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(),
        joint.command_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.command_interfaces[1].name != hardware_interface::HW_IF_POSITION) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[1].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces.size() != 3) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' has %zu state interface. 3 expected.", joint.name.c_str(),
        joint.state_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_POSITION) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
      RCLCPP_FATAL(
        rclcpp::get_logger("system_hardware"),
        "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT);
      return hardware_interface::CallbackReturn::ERROR;
    }

    hardware_node_->init_hardware(joint.name, index++);
  }

  hardware_node_->reset_device_status();
  return CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn SystemHardware::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> SystemHardware::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    HWDevice* device = hardware_node_->find_device(info_.joints[i].name);
    if (device) {
      RCLCPP_INFO_STREAM(rclcpp::get_logger("system_hardware"), "export_state_interfaces：" << info_.joints[i].name);
      state_interfaces.emplace_back(hardware_interface::StateInterface(
        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &device->velocity));
      state_interfaces.emplace_back(hardware_interface::StateInterface(
        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &device->position));
      state_interfaces.emplace_back(hardware_interface::StateInterface(
        info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &device->effort));
    } else {
      RCLCPP_ERROR(rclcpp::get_logger("system_hardware"), 
        "Failed export state interfaces: %s", info_.joints[i].name.c_str());
    }
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> SystemHardware::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    HWDevice* device = hardware_node_->find_device(info_.joints[i].name);
    if (device) {
      command_interfaces.emplace_back(hardware_interface::CommandInterface(
        // TODO(anyone): insert correct interfaces
        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &device->velocity_command));
      command_interfaces.emplace_back(hardware_interface::CommandInterface(
        // TODO(anyone): insert correct interfaces
        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &device->position_command));
    } else {
      RCLCPP_ERROR(rclcpp::get_logger("system_hardware"), 
        "Failed export command interfaces: %s", info_.joints[i].name.c_str());
    }
  }

  return command_interfaces;
}

hardware_interface::CallbackReturn SystemHardware::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to receive commands
  RCLCPP_INFO(rclcpp::get_logger("system_hardware"), "Activating ...please wait...");
  hardware_node_->setup();

  RCLCPP_INFO(rclcpp::get_logger("system_hardware"), "Successfully activated!");
  return CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn SystemHardware::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // TODO(anyone): prepare the robot to stop receiving commands
  RCLCPP_INFO(rclcpp::get_logger("system_hardware"), "Deactivating ...please wait...");
  // END: This part here is for exemplary purposes - Please do not copy to your production code
  
  RCLCPP_INFO(rclcpp::get_logger("system_hardware"), "Successfully deactivated!");
  return CallbackReturn::SUCCESS;
}

hardware_interface::return_type SystemHardware::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
  // TODO(anyone): read robot states
  if(!hardware_node_->read())
    return hardware_interface::return_type::ERROR;
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type SystemHardware::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // TODO(anyone): write robot's commands'
  if (!hardware_node_->write())
    return hardware_interface::return_type::ERROR;
  return hardware_interface::return_type::OK;
}

} // end namespace

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  hardware::SystemHardware, hardware_interface::SystemInterface)
